//
// Created by Win10 on 2023/3/29.
//

#include "foc.h"
#include "cordic.h"
#include "tim.h"
#include "drv8311p.h"
#include "mt6816ct-std.h"

static CORDIC_ConfigTypeDef CORDIC_Sin_Config;


void FOC_Bsp_Init(){
  CORDIC_Sin_Config.Function=CORDIC_FUNCTION_SINE;
  CORDIC_Sin_Config.Scale=CORDIC_SCALE_0;
  CORDIC_Sin_Config.InSize=CORDIC_INSIZE_32BITS;
  CORDIC_Sin_Config.OutSize=CORDIC_OUTSIZE_32BITS;
  CORDIC_Sin_Config.NbWrite=CORDIC_NBWRITE_1;
  CORDIC_Sin_Config.NbRead=CORDIC_NBREAD_2;
  CORDIC_Sin_Config.Precision=CORDIC_PRECISION_6CYCLES;
  HAL_CORDIC_Configure(&hcordic,&CORDIC_Sin_Config);
}

/*-------------------SINE_COSINE-------------------*/

/**
 * @brief  将数据转换成CORDIC要求的Q1.31整型数据格式。
 * @param  需要转换的数据，取值可以为负数。
 * @param  比例系数；数据除以比例系数之后再转换格式；CORDIC的输入参数数值在[-1,1]之间，故需要先除以一个比例系数。
 * @ref STM32G4 Series advanced Arm-based 32-bit MCUs - Reference Manual
 * @retval Q1.31整型数据  */
int Value_To_CORDIC31(float Value, float Coefficient) {
  int CORDIC31;
  CORDIC31 = (int) ((Value / Coefficient) * 0x80000000);
  return CORDIC31;
}

/**
 * @brief  将数据转换成CORDIC要求的Q1.15整型数据格式。
 * @param  需要转换的数据，取值可以为负数;该数据存放在高16位。
 * @param  需要转换的数据，取值可以为负数;该数据存放在低16位。
 * @param  比例系数；数据除以比例系数之后再转换格式；CORDIC的输入参数数值在[-1,1]之间，故需要先除以一个比例系数。
 * @ref STM32G4 Series advanced Arm-based 32-bit MCUs - Reference Manual
 * @retval Q1.15整型数据  */
int Value_To_CORDIC15(float ValueA, float ValueB, float Coefficient) {
  int CORDIC15;
  CORDIC15 = (int) ((ValueA / Coefficient) * 0x8000) << 16;
  CORDIC15 = CORDIC15 | (int) ((ValueB / Coefficient) * 0x8000);
  return CORDIC15;
}

/**
 * @brief  将CORDIC输出的Q1.31整型数据转换成带符号的浮点数值格式。
 * @param  需要转换的数据。
 * @param  存放输出数据的地址。
 * @note   转换无精度损失；精度要求不高时，可以将double类型换成float类型以节约RAM空间；此时精度将下降至1/10000000。  */
void CORDIC31_To_Value(int CORDIC31, float *RES) {
  if (CORDIC31 & 0x80000000) { /*为负数*/
    CORDIC31 = CORDIC31 & 0x7FFFFFFF;
    *RES = (((float) (CORDIC31) - 0x80000000) / 0x80000000);
  } else {/*为正数*/
    *RES = (float) (CORDIC31) / 0x80000000;
  }
}

/**
 * @brief  将CORDIC输出的Q1.15整型数据转换成两个带符号的浮点数值格式。
 * @param  需要转换的数据。
 * @param  存放输出数据的地址；输出高位数据。
 * @param  存放输出数据的地址；输出低位数据。  */
void CORDIC15_To_Value(int CORDIC15, float *REA, float *REB) {
  if (CORDIC15 & 0x80000000) {//处理高16位
    /*为负数*/
    *REA = ((float) ((CORDIC15 >> 16) & 0x7FFF) - 0x8000) / 0x8000;
  } else {/*为正数*/
    *REA = (float) ((CORDIC15 >> 16) & 0xFFFF) / 0x8000;
  }
  if (CORDIC15 & 0x8000) {//处理低16位
    /*为负数*/
    *REB = ((float) (CORDIC15 & 0x7FFF) - 0x8000) / 0x8000;
  } else {/*为正数*/
    *REB = (float) (CORDIC15 & 0xFFFF) / 0x8000;
  }
}

void FOC_SinCos(float theta, float *sin, float *cos){
  static int32_t input[1];
  input[0]=Value_To_CORDIC31(theta,FOC_ANGLE_MAX);
  static int32_t output[2];
  HAL_CORDIC_Calculate_DMA(&hcordic,input,output,1,CORDIC_DMA_DIR_IN_OUT);
  CORDIC31_To_Value(output[0],sin);
  CORDIC31_To_Value(output[1],cos);
}

/*-------------------PWM-------------------*/

void FOC_Set_PWM(float pwm_a, float pwm_b, float pwm_c){
  DRV_Set_PWMG_Duty((uint16_t)((float)DRV_PWM_PRD_T*pwm_a),
                    (uint16_t)((float)DRV_PWM_PRD_T*pwm_b),
                    (uint16_t)((float)DRV_PWM_PRD_T*pwm_c),DRV_DEVICE_ID_0);
}

void FOC_Set_ADC_Trig(float trig_phase){
  volatile uint16_t time = (uint16_t)(800.f*trig_phase);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, time+1);
}
/*-------------------Angle-------------------*/

float FOC_Get_Angle(){
  MT6816_Data_t mt_date;
  MT6816_Get_Queue_Head(&mt_date);
  // 重置0点并映射到[-8191,8192]
  int angle=(int)mt_date.angle-ENCODER_ANGLE_OFFSET;
  if(angle>(-MT6816_ANGLE_MAX/2)){
    if(angle>MT6816_ANGLE_MAX/2){
      angle-=MT6816_ANGLE_MAX;
    }
  }
  else angle+=MT6816_ANGLE_MAX;

  return (float)angle*360.f/((float)MT6816_ANGLE_MAX);
}

/*-------------------Current-------------------*/

void FOC_Get_Current(float* ia,float* ib,float* ic){
  *ia=(DRV_Get_ADC(DRV_C_SOA)-DRV_ADC_BASE_A)*ADC_REF_V/ADC_REF_VALUE;
  *ib=(DRV_Get_ADC(DRV_C_SOB)-DRV_ADC_BASE_B)*ADC_REF_V/ADC_REF_VALUE;
  *ic=(DRV_Get_ADC(DRV_C_SOC)-DRV_ADC_BASE_C)*ADC_REF_V/ADC_REF_VALUE;
}